﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Threading;

using Graphene.Storing;

namespace Graphene.Processing
{
    public class Processor3D : AbstractMultiSourceProcessor
    {
        public static readonly float EPSILON = (1.0E-7F);
        private static readonly int SLEEP_TIME = 80;

        public Processor3D(Model model, MultiColorImageProcessor processorSourcePrimary, MultiColorImageProcessor processorSourceSecondary /* this is a new idea, continued later> Dictionary<string, string> markerPairs */)
        {
            base.model = model;
            base.myName = processorSourcePrimary.MyName + "&" + processorSourceSecondary.MyName + "-3D_processor";
            base.myId = processorSourcePrimary.MyId + "&" + processorSourceSecondary.MyId;

            this.primaryProcessorId = processorSourcePrimary.MyId;
            this.secondaryProcessorId = processorSourceSecondary.MyId;            
            base.processorSources = new Dictionary<string, ProcessorSource>();
            base.processorSources.Add(this.primaryProcessorId, (ProcessorSource)processorSourcePrimary);
            base.processorSources.Add(this.secondaryProcessorId, (ProcessorSource)processorSourceSecondary);

            base.copiedSourceDatas = new Dictionary<string, Object>();            
            this.sourcesChanged = new Dictionary<string, Dictionary<string, bool>>();            
            this.sourceChangedLocks = new Dictionary<string, Dictionary<string, Object>>();                        
            base.resultChanged = new Dictionary<string, bool>();

            /*this is a new idea, continued later */
            int nMarkersInPrimary = ((Dictionary<string, MarkerInImage>)processorSourcePrimary.ResultData).Count;
            int nMarkersInSecondary = ((Dictionary<string, MarkerInImage>)processorSourceSecondary.ResultData).Count;            

            /* Creating the Markers. They need id, name, some color. */
            int nmP = ((Dictionary<string, MarkerInImage>)processorSourcePrimary.ResultData).Count;
            int nmS = ((Dictionary<string, MarkerInImage>)processorSourceSecondary.ResultData).Count;            
            string[] idsP = new string[nmP];
            string[] namesP = new string[nmP];
            Color[] colorP = new Color[nmP];
            Dictionary<string, bool> localChangedInPrimary = new Dictionary<string, bool>();
            Dictionary<string, Object> localChangedLockInPrimary = new Dictionary<string, object>();
            Dictionary<string, MarkerInImage> localCopiedSourceInPrimary = new Dictionary<string, MarkerInImage>();
            int jP = 0;
            foreach (MarkerInImage localMarkerInPrimary in ((Dictionary<string, MarkerInImage>)processorSourcePrimary.ResultData).Values)
            {                
                idsP[jP] = localMarkerInPrimary.MyId;
                namesP[jP] = localMarkerInPrimary.MyName;
                colorP[jP] = localMarkerInPrimary.RepresentingColor;

                /* Adding the state of 'has-changed?' for each of the markers */
                localChangedInPrimary.Add(localMarkerInPrimary.MyId, false);

                /* Adding a lock for every 'has-changed?' property. */
                localChangedLockInPrimary.Add(localMarkerInPrimary.MyId, new Object());

                /* Adding a copied instance of the result of the source. */
                localCopiedSourceInPrimary.Add(localMarkerInPrimary.MyId, (MarkerInImage)localMarkerInPrimary.Clone());

                jP++;
            }

            /* Adding the state of 'has-changed?' for each of the markers in each sources. */
            this.sourcesChanged.Add(primaryProcessorId, localChangedInPrimary);

            /* Adding the locks. */
            this.sourceChangedLocks.Add(primaryProcessorId, localChangedLockInPrimary);

            /* Adding the copied sources. */
            this.copiedSourceDatas.Add(primaryProcessorId, localCopiedSourceInPrimary);

            string[] idsS = new string[nmS];
            string[] namesS = new string[nmS];
            //Color[] colorS = new Color[nmS];
            Dictionary<string, bool> localChangedInSecondary = new Dictionary<string, bool>();
            Dictionary<string, Object> localChangedLockInSecondary = new Dictionary<string, object>();
            Dictionary<string, MarkerInImage> localCopiedSourceInSecondary = new Dictionary<string, MarkerInImage>();
            int jS = 0;
            foreach (MarkerInImage localMarkerInSecondary in ((Dictionary<string, MarkerInImage>)processorSourceSecondary.ResultData).Values)
            {
                idsS[jS] = localMarkerInSecondary.MyId;
                namesS[jS] = localMarkerInSecondary.MyName;
                //colorS[jS] = localMarkerInSecondary.RepresentingColor;

                /* Adding the state of 'has-changed?' for each of the markers */
                localChangedInSecondary.Add(localMarkerInSecondary.MyId, false);

                /* Adding a lock for every 'has-changed?' property. */
                localChangedLockInSecondary.Add(localMarkerInSecondary.MyId, new Object());

                /* Adding a copied instance of the result of the source. */
                localCopiedSourceInSecondary.Add(localMarkerInSecondary.MyId, (MarkerInImage)localMarkerInSecondary.Clone());

                jS++;
            }

            /* Adding the state of 'has-changed?' for each of the markers in each sources. */
            this.sourcesChanged.Add(secondaryProcessorId, localChangedInSecondary);

            /* Adding the locks for the secondary processor. */
            this.sourceChangedLocks.Add(secondaryProcessorId, localChangedLockInSecondary);

            /* Adding the copied sources. */
            this.copiedSourceDatas.Add(secondaryProcessorId, localCopiedSourceInSecondary);

            int nm = nmS;
            if (nmP > nmS) 
            {
                nm = nmP;
            }
            this.marker2DToMarker3D = new Dictionary<string, string>();
            Dictionary<string, MarkerInSpace> markers = new Dictionary<string, MarkerInSpace>();
            for (int i = 0; i < nm; i++)
            {
                string newId = idsP[i] + "&" + idsS[i];
                string newName = namesP[i] + "&" + namesS[i];
                MarkerInSpace marker = new MarkerInSpace(newId, newName, colorP[i]);
                markers.Add(marker.MyId, marker);
          
                /* Adding the state of 'has-changed?' for the resulting markers. */
                ((Dictionary<string, bool>)resultChanged).Add(marker.MyId, false);

                /* Making the dependency between the source marker and our own marker. */
                if (!this.marker2DToMarker3D.ContainsKey(idsP[i]))
                {
                    this.marker2DToMarker3D.Add(idsP[i], newId);
                }
                if (!this.marker2DToMarker3D.ContainsKey(idsS[i]))
                {
                    this.marker2DToMarker3D.Add(idsS[i], newId);
                }
            }
            base.result = markers;            

            base.averageProcessTime = 0.0;
            base.longestProcessTime = 0;
        }

        public override void MyRun()
        {
            while (true)
            {                
                for (int i = 0; i < ((Dictionary<string, MarkerInSpace>)this.result).Count; i++)
                {
                    string markerIdPrimary = this.sourcesChanged[primaryProcessorId].ElementAt(i).Key;
                    string markerIdSecondary = this.sourcesChanged[secondaryProcessorId].ElementAt(i).Key;
                    bool changedPrimary = this.sourcesChanged[primaryProcessorId][markerIdPrimary];
                    bool changedSecondary = this.sourcesChanged[secondaryProcessorId][markerIdSecondary];
                    Object changedLockPrimary = this.sourceChangedLocks[primaryProcessorId][markerIdPrimary];
                    Object changedLockSecondary = this.sourceChangedLocks[secondaryProcessorId][markerIdSecondary];

                    if (changedPrimary || changedSecondary)
                    {
                        if (changedPrimary)
                        {
                            lock (changedLockPrimary)
                            {
                                changedPrimary = false;
                            }
                        }
                        if (changedSecondary)
                        {
                            lock (changedLockSecondary)
                            {
                                changedSecondary = false;
                            }
                        }

                        /* Copying. */
                        MarkerInImage markerFromPrimary = ((Dictionary<string, MarkerInImage>)this.copiedSourceDatas[primaryProcessorId])[markerIdPrimary] = (MarkerInImage)((Dictionary<string, MarkerInImage>)(this.processorSources[primaryProcessorId].ResultData))[markerIdPrimary].Clone();
                        MarkerInImage markerFromSecondary = ((Dictionary<string, MarkerInImage>)this.copiedSourceDatas[secondaryProcessorId])[markerIdSecondary] = (MarkerInImage)((Dictionary<string, MarkerInImage>)(this.processorSources[secondaryProcessorId].ResultData))[markerIdSecondary].Clone();

                        /* Computing. */                        
                        string marker3DId = this.marker2DToMarker3D[markerIdPrimary];
                        MarkerInSpace marker3D = ((Dictionary<string, MarkerInSpace>)this.result)[marker3DId];                                                                       
                        marker3D.Position = See(markerFromPrimary.Position, markerFromSecondary.Position);                      

                        /* Set necessary result to changed. */
                        ((Dictionary<string, bool>)this.resultChanged)[marker3DId] = true;

                        /* Notify. */
                        if (this.resultChangedCall != null)
                        {
                            this.resultChangedCall(this, resultChanged);
                        }
                        if (this.processedCall != null)
                        {
                            this.processedCall(this);
                        }
                    }
                }
                try
                {
                    Thread.Sleep(SLEEP_TIME);
                }
                catch (ThreadInterruptedException ex)
                {
                    Console.WriteLine("whoooops");
                    break;
                }
            }
        }

        /**
         * Sets an item in the sourcesChanged variable to true, so that we know that if we have
         * something dependent on the corresponding item in sourceDatas we'll hhave to process the new source.         
         * This sourcesChanged variable is used to prevent processing the same source over and
         * over if that would be the case in MyRun().
         */
        public override void SetSourceToChanged(object sender, Object whatChanged)
        {
            /*
            lock (fasz)
            {
             */ 
                if (sender.Equals(this.processorSources[primaryProcessorId]))
                {
                    foreach (string markerId in ((Dictionary<string, bool>)whatChanged).Keys)
                    {
                        if (((Dictionary<string, bool>)whatChanged)[markerId])
                        {
                            lock (this.sourceChangedLocks[primaryProcessorId][markerId])
                            {
                                this.sourcesChanged[primaryProcessorId][markerId] = true;
                            }
                        }
                    }
                }
                else if (sender.Equals(this.processorSources[secondaryProcessorId]))
                {
                    foreach (string markerId in ((Dictionary<string, bool>)whatChanged).Keys)
                    {
                        if (((Dictionary<string, bool>)whatChanged)[markerId])
                        {
                            lock (this.sourceChangedLocks[secondaryProcessorId][markerId])
                            {
                                this.sourcesChanged[secondaryProcessorId][markerId] = true;
                            }
                        }
                    }
                }
            /*
            }
             */ 
        }

        public override void AddResultChangedHandler(Graphene.Processing.ResultChangedHandler handler)
        {
            this.resultChangedCall += handler;
        }

        public override void RemoveResultChangedHandler(Graphene.Processing.ResultChangedHandler handler)
        {
            this.resultChangedCall -= handler;
        }

        public override void AddProcessedHandler(Graphene.Processing.ProcessedHandler handler)
        {
            this.processedCall += handler;
        }

        public override void RemoveProcessedHandler(Graphene.Processing.ProcessedHandler handler)
        {
            this.processedCall -= handler;
        }

        /**
         * With the use of two two dimensional coordinates, and the positions of the cameras
         * this method computes the 3D position of the given point.
         */
        private PointReal3D See(PointReal2D markerPositionOfPrimary, PointReal2D markerPositionOfSecondary)
        {
            VideoSource videoSourceOfPrimary = ((VideoSource)((MultiColorImageProcessor)processorSources[primaryProcessorId]).AssignedProcessorSource);
            VideoSource videoSourceOfSecondary = ((VideoSource)((MultiColorImageProcessor)processorSources[secondaryProcessorId]).AssignedProcessorSource);            
                    
            /* Direction of the beam. */
            Microsoft.DirectX.Vector3 directionOfPrimary = new  Microsoft.DirectX.Vector3(1, 0, 0);
            Microsoft.DirectX.Vector3 normalYOfPrimary = new  Microsoft.DirectX.Vector3(0, 1, 0);
            Microsoft.DirectX.Vector3 normalZOfPrimary = new  Microsoft.DirectX.Vector3(0, 0, 1);
                                    
            Microsoft.DirectX.Matrix rotationMatrix;
            rotationMatrix = Microsoft.DirectX.Matrix.RotationZ(videoSourceOfPrimary.AngleInWorld.X);
            rotationMatrix.Multiply(Microsoft.DirectX.Matrix.RotationY(videoSourceOfPrimary.AngleInWorld.Y));
            directionOfPrimary.TransformCoordinate(rotationMatrix);
            normalYOfPrimary.TransformCoordinate(rotationMatrix);
            normalZOfPrimary.TransformCoordinate(rotationMatrix);
                
            PointReal2D beamDirectionOfPrimary = new PointReal2D(-markerPositionOfPrimary.Y * videoSourceOfPrimary.FOV.Y / 2, -markerPositionOfPrimary.X * videoSourceOfSecondary.FOV.X / 2);                  
            rotationMatrix = Microsoft.DirectX.Matrix.RotationAxis(normalZOfPrimary, beamDirectionOfPrimary.X);
            rotationMatrix.Multiply(Microsoft.DirectX.Matrix.RotationAxis(normalYOfPrimary, beamDirectionOfPrimary.Y));                                        
            directionOfPrimary.TransformCoordinate(rotationMatrix);

            Processor3D.O1 = videoSourceOfPrimary.PositionInWorld.ToVector3();
            Processor3D.D1 = Processor3D.O1 + directionOfPrimary;

            /* Direction of the beam. */
            Microsoft.DirectX.Vector3 directionOfSecondary = new  Microsoft.DirectX.Vector3(1, 0, 0);
            Microsoft.DirectX.Vector3 normalYOfSecondary = new  Microsoft.DirectX.Vector3(0, 1, 0);
            Microsoft.DirectX.Vector3 normalZOfSecondary = new  Microsoft.DirectX.Vector3(0, 0, 1);
                                                
            rotationMatrix = Microsoft.DirectX.Matrix.RotationZ(videoSourceOfSecondary.AngleInWorld.X);
            rotationMatrix.Multiply(Microsoft.DirectX.Matrix.RotationY(videoSourceOfSecondary.AngleInWorld.Y));
            directionOfSecondary.TransformCoordinate(rotationMatrix);
            normalYOfSecondary.TransformCoordinate(rotationMatrix);
            normalZOfSecondary.TransformCoordinate(rotationMatrix);
                
            PointReal2D beamDirectionOfSecondary = new PointReal2D(-markerPositionOfSecondary.Y * videoSourceOfSecondary.FOV.Y / 2, -markerPositionOfSecondary.X * videoSourceOfSecondary.FOV.X / 2);                  
            rotationMatrix = Microsoft.DirectX.Matrix.RotationAxis(normalZOfSecondary, beamDirectionOfSecondary.X);
            rotationMatrix.Multiply(Microsoft.DirectX.Matrix.RotationAxis(normalYOfSecondary, beamDirectionOfSecondary.Y));                                        
            directionOfSecondary.TransformCoordinate(rotationMatrix);

            Processor3D.O2 = videoSourceOfSecondary.PositionInWorld.ToVector3();
            Processor3D.D2 = Processor3D.O2 + directionOfSecondary;
            
            Microsoft.DirectX.Vector3[] closestPoints = SkewLinesClosestPoints(
                videoSourceOfPrimary.PositionInWorld.ToVector3(),
                videoSourceOfSecondary.PositionInWorld.ToVector3(),
                directionOfPrimary,
                directionOfSecondary
                );

            Processor3D.C1 = closestPoints[0];
            Processor3D.C2 = closestPoints[1];

            Processor3D.CROSS = Microsoft.DirectX.Vector3.Cross(directionOfPrimary, directionOfSecondary);
            Processor3D.CROSS.Normalize();

            Microsoft.DirectX.Vector3 middlePoint = closestPoints[0] + closestPoints[1];
            middlePoint.Multiply(0.5f);
            
            return new PointReal3D(middlePoint.X, middlePoint.Y, middlePoint.Z);            
        }

        public static Microsoft.DirectX.Vector3 O1;
        public static Microsoft.DirectX.Vector3 O2;
        public static Microsoft.DirectX.Vector3 D1;
        public static Microsoft.DirectX.Vector3 D2;
        public static Microsoft.DirectX.Vector3 C1;
        public static Microsoft.DirectX.Vector3 C2;
        public static Microsoft.DirectX.Vector3 CROSS;

        public static Microsoft.DirectX.Vector4 PlaneByAPointAndTwoVectors(Microsoft.DirectX.Vector3 P, Microsoft.DirectX.Vector3 u, Microsoft.DirectX.Vector3 v)
        {
            Microsoft.DirectX.Vector3 n = Microsoft.DirectX.Vector3.Cross(u, v);
            n.Normalize();
            float d = -(n.X * P.X + n.Y * P.Y + n.Z * P.Z);
            Microsoft.DirectX.Vector4 plane_P_u_v = new Microsoft.DirectX.Vector4(n.X, n.Y, n.Z, d);            

            return plane_P_u_v;
        }

        public static float DistancePointToPlane(Microsoft.DirectX.Vector3 point, Microsoft.DirectX.Vector4 plane)
        {
            return (point.X * plane.X + point.Y * plane.Y + point.Z * point.Z + plane.W); 
        }

        public static Microsoft.DirectX.Vector3 PointByIntersectionOfPlaneAndLineByTwoPoints(Microsoft.DirectX.Vector4 plane, Microsoft.DirectX.Vector3 pointAThroughLine, Microsoft.DirectX.Vector3 pointBThroughLine)
        {
            Microsoft.DirectX.Vector3 intersectionPoint;
            float distance_A_plane = DistancePointToPlane(pointAThroughLine, plane);
            float distance_B_plane = DistancePointToPlane(pointBThroughLine, plane);
            DIST1 = distance_A_plane;
            DIST2 = distance_B_plane;

            float denom = distance_A_plane - distance_B_plane;
            if (Math.Abs(denom / (Math.Abs(distance_A_plane) + Math.Abs(distance_B_plane))) < EPSILON)
            {
                //no intersection. Parallel.
                intersectionPoint = new Microsoft.DirectX.Vector3();
            }
            else
            {
                float t = distance_A_plane / denom;
                float x = t * pointBThroughLine.X + (1 - t) * pointAThroughLine.X;
                float y = t * pointBThroughLine.Y + (1 - t) * pointAThroughLine.Y;
                float z = t * pointBThroughLine.Z + (1 - t) * pointAThroughLine.Z;
                intersectionPoint = new Microsoft.DirectX.Vector3(x, y, z);                
            }

            return intersectionPoint;
        }

        public static Microsoft.DirectX.Vector3 PointByIntersectionOfPlaneAndLineByPointAndDirection(Microsoft.DirectX.Vector4 plane, Microsoft.DirectX.Vector3 pointThroughLine, Microsoft.DirectX.Vector3 directionOfLine)
        {
            Microsoft.DirectX.Vector3 pointBThroughLine = pointThroughLine + directionOfLine;
            return PointByIntersectionOfPlaneAndLineByTwoPoints(plane, pointThroughLine, pointBThroughLine);
        }

        public static Microsoft.DirectX.Vector3 N;
        public static Microsoft.DirectX.Vector3 INTER1;
        public static Microsoft.DirectX.Vector3 INTER2;
        public static Microsoft.DirectX.Vector4 PLANE1;
        public static Microsoft.DirectX.Vector4 PLANE2;
        public static float DIST1;
        public static float DIST2;

        /* @param N Normal of the plane
         * @param P3 A point on the plane
         * @param P1 A point on the line
         * @param P2 Another point on the line
         */
        public static Microsoft.DirectX.Vector3 PointByIntersectionOfPlaneAndLine(
            Microsoft.DirectX.Vector3 N,
            Microsoft.DirectX.Vector3 P3,
            Microsoft.DirectX.Vector3 P1,
            Microsoft.DirectX.Vector3 P2)
        {
            if (Microsoft.DirectX.Vector3.Dot(N, P2 - P1) == 0)
            {
                //parallel. Throw exception.
                return new Microsoft.DirectX.Vector3();
            }
            float u = Microsoft.DirectX.Vector3.Dot(N, P3 - P1) / Microsoft.DirectX.Vector3.Dot(N, P2 - P1);
            Microsoft.DirectX.Vector3 D = (P2 - P1);
            D.Multiply(u);
            Microsoft.DirectX.Vector3 intersection = P1 + D;

            return intersection;
        }

        public static Microsoft.DirectX.Vector3[] SkewLinesClosestPoints(Microsoft.DirectX.Vector3 A,
            Microsoft.DirectX.Vector3 B,
            Microsoft.DirectX.Vector3 vA,
            Microsoft.DirectX.Vector3 vB
            )
        {
            Microsoft.DirectX.Vector3 vN = Microsoft.DirectX.Vector3.Cross(vA, vB);
            vN.Normalize();
            Microsoft.DirectX.Vector4 plane_A_vA_vN = PlaneByAPointAndTwoVectors(A, vA, vN);
            Microsoft.DirectX.Vector4 plane_B_vB_vN = PlaneByAPointAndTwoVectors(B, vB, vN);
            PLANE1 = plane_A_vA_vN;
            PLANE2 = plane_B_vB_vN;
            Microsoft.DirectX.Vector3 normal_plane_A = new Microsoft.DirectX.Vector3(plane_A_vA_vN.X, plane_A_vA_vN.Y, plane_A_vA_vN.Z);
            Microsoft.DirectX.Vector3 normal_plane_B = new Microsoft.DirectX.Vector3(plane_B_vB_vN.X, plane_B_vB_vN.Y, plane_B_vB_vN.Z);
            Microsoft.DirectX.Vector3 intersectionPoint_a_vB = PointByIntersectionOfPlaneAndLine(normal_plane_A, A, B, (B + vB));
            Microsoft.DirectX.Vector3 intersectionPoint_b_vA = PointByIntersectionOfPlaneAndLine(normal_plane_B, B, A, (A + vA));            
            INTER1 = intersectionPoint_a_vB;
            INTER2 = intersectionPoint_b_vA;

            Microsoft.DirectX.Vector3[] closestPoints = new Microsoft.DirectX.Vector3[2];
            closestPoints[0] = intersectionPoint_a_vB;
            closestPoints[1] = intersectionPoint_b_vA;
            return closestPoints;
        }

        /* Attributes: */

        private string primaryProcessorId;
        private string secondaryProcessorId;       

        /**
         * A lock to keep the sourcesChanged attribute safe from concurrent threads.
         */
        private Dictionary<string, Dictionary<string, Object>> sourceChangedLocks;

        /**
         * This is set to true whenever the incoming data, the source, has changed
         * in the corresponding ProcessorSource.
         */
        private Dictionary<string, Dictionary<string, bool>> sourcesChanged;

        /* Function that translates the source marker to the (only)one result marker that is 
         * dependent on it. Mainly a solution for the difference in the ids. */
        private Dictionary<string, string> marker2DToMarker3D;

        /** Methods (delegates) added to this attribute will be called whenever the results change.
         * This is source for the next processor after this one. */
        private event Graphene.Processing.ResultChangedHandler resultChangedCall;

        private event Graphene.Processing.ProcessedHandler processedCall;
    }
}
